/**
  ******************************************************************************
  * @file     : can_Motor_class.cpp
  * @brief    : can电机类源文件
  ******************************************************************************
  * @attention
  * 对外接口:
  * 	set_speed函数让电机以无角度环模式的速度环运行
  *     set_rel_angle让电机转过指定的角度
  *     set_abs_angle让电机转到指定的角度
  *     set_stop让电机停止输出
  *     update_measure更新父类Can_motor_measure的反馈数据,通过多态为不同派生类提供不一样的实现
  *     Can_motor_measure.can_msg_que目前用于在bsp_can.cpp里面通过队列来传递数据
  * 配置接口:
  *     请调用父类Can_motor_measure.measure_config函数配置电机的初始模式以及can ID
  *     调用成员spid和apid的pid_param_init函数初始化pid参数 
  *******************************************************************************/

#include "can_motor_class.h"
#include "bsp_can.h"
/*#########################################################*/
/*           Can_motor_measure类的成员函数定义区              */
/*#########################################################*/
/**
 * @brief 配置指定电机的参数
 * @param state			电机运转状态		    @args RUN 或者 STOP
 * @param angle_mode	角度环状态			@args ANGLE_REL 或 ANGLE_ABS 或 ANGLE_NO
 * @param stdID			电机的标准canID		@args 0x000~0x7FF
 */
void Can_motor_measure :: measure_config(
        MOTOR_STATE state,
        MOTOR_STATE angle_mode,
        uint32_t stdID
){
    this->state = state;
    this->angle_mode = angle_mode;
    this->stdID = stdID;
}

Can_motor_measure::Can_motor_measure()
{
    this-> stdID = 0;
    this-> target_speed = 0.0f;
    this-> target_angle = 0.0f;
    this-> speed_rpm_now = 0.0f;
    this-> speed_rpm_last = 0.0f;
    this-> angle_now = 0.0f;
    this-> angle_last = 0.0f;
    this-> angle_total = 0.0f;
    this-> state = MOTOR_STATE::STOP;
    this-> angle_mode = MOTOR_STATE::ANGLE_NO;
}

/*#########################################################*/
/*              Can_motor类的成员函数定义区                   */
/*#########################################################*/

/**
 * @brief 	(点动式函数)让电机以指定速度转(无角度环模式)
 * @param 	speed	转速 单位rpm
 */
void Can_motor::set_speed(float speed)
{
    if(this -> state != MOTOR_STATE:: RUN) this -> state = RUN;
    this -> angle_mode = MOTOR_STATE:: ANGLE_NO;		//无角度环模式
    this -> target_speed = speed;
}

/**
 * @brief 	(点动式函数)让电机转过指定角度（速度环内环,角度环外环pid）
 * @param 	angle		角度(角度制)	@args 任意实数
 */
void Can_motor::set_rel_angle(float angle)
{
    if(this -> state != MOTOR_STATE:: RUN) this -> state = RUN;
    this -> angle_mode = MOTOR_STATE:: ANGLE_REL;
    //让电机转过指定角度
    this -> target_angle = this -> angle_now + angle;
    this -> angle_total =  this -> angle_now;
}

/**
 * @brief 	(点动式函数)让电机转到指定角度（速度环内环,角度环外环pid）
 * @param 	angle		角度(角度制)	@args 任意实数
 */
void Can_motor::set_abs_angle(float angle)
{
    if(this -> state != MOTOR_STATE:: RUN) this -> state = RUN;
    this -> angle_mode = MOTOR_STATE:: ANGLE_REL;
    //让电机转到指定角度
    this -> target_angle = angle;
    this -> angle_total =  this -> angle_now;
}

/**
 * @brief 	(点动式函数)停止对指定电机输出电流/电压
 */
void Can_motor::set_stop()
{
    //将一切模式清零
    this -> state = MOTOR_STATE::STOP;			//电机停止输出
    this -> angle_mode = MOTOR_STATE::ANGLE_NO;	//无角度环模式

    this -> target_speed = 0;		//清除目标速度
    this -> target_angle = 0;		//清除目标角度
    this -> angle_total = this -> angle_now;	//重置总转过角度
}

/**
 * @brief 	更新父类的反馈数据兼电机的实时参数,基于rtos的消息队列,消息队列在can接收中断回调里将原始报文入队,
 *          根据不同的基于Can_motor类的派生类有不同的实现
 * @note    后续如果要新增新的电机,请添加新的派生类继承基类Can_motor,并且重写此函数
 */
void Can_motor::update_measure()
{
    //默认为 DJI3508 和 DJI2006 还有 DJI6020
    //只获取转子角度(解算为角度值),转速(rpm)
    can_recive_msg_t temp_msg;
    //从队列中取数据,不等待
    osStatus_t qstatus = osMessageQueueGet(this->can_msg_que, &temp_msg, NULL, 0);
    if(qstatus == osOK)
    {
        //更新角度
        this->angle_last = this->angle_now;
        int16_t agnow = (int16_t)temp_msg.data[0] << 8 | temp_msg.data[1];
        this->angle_now = (float)agnow * 360.0f / 8192.0f;
        //更新转速
        this->speed_rpm_last = this->speed_rpm_now;
        int16_t spnow = (int16_t)temp_msg.data[2] << 8 | temp_msg.data[3];
        this->speed_rpm_now = spnow;
    }
}
#if 0
/**
 * @brief 	DJI_3508类measure_update的重写
 */
void DJI_3508::update_measure ()
{
    //只获取转子角度(解算为角度值),转速(rpm)
    can_recive_msg_t temp_msg;
    //从队列中取数据,不等待
    osStatus_t qstatus = osMessageQueueGet(this->can_msg_que, &temp_msg, NULL, 0);
    if(qstatus == osOK)
    {
        //更新角度
        this->angle_last = this->angle_now;
        int16_t agnow = (int16_t)temp_msg.data[0] << 8 | temp_msg.data[1];
        this->angle_now = (float)agnow * 360.0f / 8192.0f;
        //更新转速
        this->speed_rpm_last = this->speed_rpm_now;
        int16_t spnow = (int16_t)temp_msg.data[2] << 8 | temp_msg.data[3];
        this->speed_rpm_now = spnow;
    }
}
/**
 * @brief 	DJI_2006类measure_update的重写
 */
void DJI_2006::update_measure ()
{
    //只获取转子角度(解算为角度值),转速(rpm)
    can_recive_msg_t temp_msg;
    //从队列中取数据,不等待
    osStatus_t qstatus = osMessageQueueGet(this->can_msg_que, &temp_msg, NULL, 0);
    if(qstatus == osOK)
    {
        //更新角度
        this->angle_last = this->angle_now;
        int16_t agnow = (int16_t)temp_msg.data[0] << 8 | temp_msg.data[1];
        this->angle_now = (float)agnow * 360.0f / 8192.0f;
        //更新转速
        this->speed_rpm_last = this->speed_rpm_now;
        int16_t spnow = (int16_t)temp_msg.data[2] << 8 | temp_msg.data[3];
        this->speed_rpm_now = spnow;
    }
}
/**
 * @brief 	DJI_6020类measure_update的重写
 */
void DJI_6020::update_measure ()
{
    //只获取转子角度(解算为角度值),转速(rpm)
    can_recive_msg_t temp_msg;
    //从队列中取数据,不等待
    osStatus_t qstatus = osMessageQueueGet(this->can_msg_que, &temp_msg, NULL, 0);
    if(qstatus == osOK)
    {
        //更新角度
        this->angle_last = this->angle_now;
        int16_t agnow = (int16_t)temp_msg.data[0] << 8 | temp_msg.data[1];
        this->angle_now = (float)agnow * 360.0f / 8192.0f;
        //更新转速
        this->speed_rpm_last = this->speed_rpm_now;
        int16_t spnow = (int16_t)temp_msg.data[2] << 8 | temp_msg.data[3];
        this->speed_rpm_now = spnow;
    }
}
#endif

/**
 * @brief 	Can_motor的构造函数
 */
Can_motor::Can_motor()
					  : can_msg_que_size(1),      //消息队列大小默认是1
						can_msg_que(NULL)        //指针先给个NULL
{
    //创建消息队列,数据类型为bsp_can.cpp中的can_recive_msg_t
    this-> can_msg_que = osMessageQueueNew(can_msg_que_size,
                                           sizeof(can_recive_msg_t),
                                           NULL);
}

